import cv2 as cv
import numpy as np
from skimage import morphology
import scipy.spatial as spt
import math
import serial
import re


# *坐标捕捉类


class Pointcapture():
    # *本类的主要功能是捕捉屏幕上的红色激光光斑、
    # * 绿色激光光斑以及黑色胶布矩形框的中心线点，并规划路线
    zero_point = (0, 0)  # 参考坐标原点
    model=0              # 模式判别量
    oldmodel = -1        # 模式记忆量
    red_hsv_hlow = 30
    red_hsv_hhigh = 32
    red_hsv_slow = 0
    red_hsv_shigh = 10
    red_hsv_vlow = 230
    red_hsv_vhigh = 255
    def __init__(self):  # 初始化类的属性
        pass
    
    def setZeroPoint(self, zpt):
        """设置参考零点位置

        Args:
            zpt (tuple): 新的坐标原点
        """
        self.zero_point = zpt

    def addtext(self, img, text, pos, color, ):
        """用于在图像上添加临时观测的文本

        Args:
            img (_type_): 要添加文本的图像载体
            text (_type_): 要添加的文本字符串
            pos (_type_): 要添加文本的位置坐标
            color (_type_): 文本颜色
            zero_point (tuple, optional): 要添加的文本的位置坐标的参考原点. Defaults to (0, 0).
        """
        font = cv.FONT_HERSHEY_SIMPLEX
        try:
            pos = (pos[0] + self.zero_point[0], pos[1] + self.zero_point[1])
        except TypeError:
            pos = (0, 0)
        cv.putText(img, text, pos, font, 1, color, 2, cv.LINE_AA)
    def __enlargeContour(self, points,length):
        M = cv.moments(points)
        centralpoint =(int(M['m10']/M['m00']),int(M['m01']/M['m00']))
        res_list = []
        for i in range(len(points)):
            try :
                th = math.atan(abs(points[i][0][1]-centralpoint[1])/abs(points[i][0][0]-centralpoint[0]))
            except ZeroDivisionError :
                if points[i][1]-centralpoint[1]>0 :
                    th = math.Pi/2
                else:
                    th = math.Pi/2
            
            if points[i][0][1]-centralpoint[1]<0 and points[i][0][0]-centralpoint[0]>0 :
                delta_x = (length+0.5)*math.cos(th)
                delta_y = -(length+0.5)*math.sin(th)
            elif points[i][0][1]-centralpoint[1]>0 and points[i][0][0]-centralpoint[0]>=0 :
                delta_x = (length+0.5)*math.cos(th)
                delta_y = (length+0.5)*math.sin(th) 
            elif points[i][0][1]-centralpoint[1]>=0 and points[i][0][0]-centralpoint[0]<0 :
                delta_x = -(length+0.5)*math.cos(th)
                delta_y = (length+0.5)*math.sin(th)
            elif points[i][0][1]-centralpoint[1]<0 and points[i][0][0]-centralpoint[0]<0 :
                delta_x = -(length+0.5)*math.cos(th)
                delta_y = -(length+0.5)*math.sin(th) 
            else :      
                delta_x = (length+0.5)*math.cos(th)
                delta_y = -(length+0.5)*math.sin(th)
            res_list.append((int(points[i][0][0]+delta_x),int(points[i][0][1]+0.5+delta_y)))
        return res_list,centralpoint
    
    
    def __endpoints_of_skeleton(self, center_line_img):
        """计算骨线图上的端点位置

        Args:
            center_line_img (skeleton_img): 要计算的骨线图

        Returns:
            list: 包含所有端点坐标元组的列表
        """
        W, H = center_line_img.shape
        img = center_line_img
        endpoints = []
        x, y = np.where(img > 0)

        for i, j in zip(x, y):
            if i - 1 < 0 or j - 1 < 0 or i + 1 >= W or j + 1 >= H:
                continue
                # 条件A 该点的四领域内仅有一个点与之连接
            c_a = (img[i - 1, j] + img[i + 1, j] +
                   img[i, j - 1] + img[i, j + 1]) == 255

            # 条件B 该点的对角领域内仅有一个点与之连接
            c_b = (img[i - 1, j - 1] + img[i - 1, j + 1] +
                   img[i + 1, j - 1] + img[i + 1, j + 1]) == 255

            # 该点的八领域内仅有一个点
            if (img[i - 1:i + 2, j - 1:j + 2] == 255).sum() <= 2:
                endpoints.append([i, j])

            # 同时满足两个条件AB，且设置八种其余的端点坐标情况，如：
            '''
            ##0  0##         
            0#0  0#0   ...
            000  000
            '''
            if c_a and c_b:
                if (img[i - 1, j - 1:j + 2] == 255).sum() == 2 or (img[i + 1, j - 1:j + 2] == 255).sum() == 2 or \
                        (img[i - 1:i + 2, j - 1] == 255).sum() == 2 or (img[i - 1:i + 2, j + 1] == 255).sum() == 2:
                    endpoints.append([i, j])
        return endpoints

    def getReddotPos(self, img, display=False):
        """获取红色光斑的坐标

        Args:
            img (wild_img): 摄像头数据或原生图片
        Returns:
            position(point): 返回红点坐标
            result_img(mark_img):标注后的图像
        """
        img = img.copy()
        kernel = np.ones((5, 5), np.uint8)
        img_hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
        # 定义HSV中掩码通过色彩的范围
        lower_val = np.array([self.red_hsv_hlow, self.red_hsv_slow, self.red_hsv_vlow])
        upper_val = np.array([self.red_hsv_hhigh, self.red_hsv_shigh, self.red_hsv_vhigh])
        # 设置HSV的阈值,得到掩码图
        mask = cv.inRange(img_hsv, lower_val, upper_val)
        # 膨胀
        dilation = cv.dilate(mask, kernel, iterations=1)
        # # 侵蚀
        erosion = cv.erode(dilation, kernel, iterations=1)
        # 二值化
        _, thresh = cv.threshold(erosion, 100, 255, 0)
        # # 得到轮廓坐标
        thresh, contours, hierarchy = cv.findContours(thresh, cv.RETR_TREE, cv.CHAIN_APPROX_NONE)
        # 得到质心坐标
        # 默认质心坐标设置
        position = (None, None)
        areas =[]
        # 获取有效质心坐标
        for each in contours:
            areas.append( cv.contourArea(each) )
        try:
            index = areas.index(max(areas))
        except ValueError:
            return position,img
        try:
            cnt = contours[index]
            M = cv.moments(cnt)
            try:
                position = (int(M['m10'] / M['m00']) - self.zero_point[0],
                            int(M['m01'] / M['m00'] - self.zero_point[1]))
            except ZeroDivisionError:
                pass
        except IndexError:
            pass
        # 绘制所用轮廓
        if display:
            result_img = cv.drawContours(img, contours, -1, (158, 168, 3), 3)
            return position, result_img
        else:
            return position

    # 绿色激光捕捉函数
    def getGreendotPos(self, img, hl, hh, sl, sh, vl, vh, display=False):
        """获取绿色光斑的坐标

        Args:
            img (wild_img): 摄像头数据或原生图片
            hl (int): 色调低阈值
            hh (int): 色调高阈值
            sl (int): 饱和度低阈值
            sh (int): 饱和度高阈值
            vl (int): 亮度低阈值
            vh (int): 亮度高阈值

        Returns:
            position(point): 返回红点坐标
            result_img(mark_img):标注后的图像
        """
        img = img.copy()
        kernel = np.ones((5, 5), np.uint8)
        img_hsv = cv.cvtColor(img, cv.COLOR_BGR2HSV)
        # 定义HSV中掩码通过色彩的范围
        lower_val = np.array([hl, sl, vl])
        upper_val = np.array([hh, sh, vh])
        # 设置HSV的阈值,得到掩码图
        mask = cv.inRange(img_hsv, lower_val, upper_val)
        # 膨胀
        dilation = cv.dilate(mask, kernel, iterations=1)
        # # 侵蚀
        erosion = cv.erode(dilation, kernel, iterations=1)
        # 二值化
        _, thresh = cv.threshold(erosion, 100, 255, 0)
        # # 得到轮廓坐标
        thresh, contours, hierarchy = cv.findContours(thresh, cv.RETR_TREE, cv.CHAIN_APPROX_NONE)
        # 得到质心坐标
        # 默认质心坐标设置
        position = (None, None)
        areas =[]
        # 获取有效质心坐标
        for each in contours:
            areas.append( cv.contourArea(each) )
        try:
            index = areas.index(max(areas))
        except ValueError:
            return position,img
        try:
            cnt = contours[0]
            M = cv.moments(cnt)
            try:
                position = (int(M['m10'] / M['m00']) - self.zero_point[0],
                            int(M['m01'] / M['m00'] - self.zero_point[1]))
            except ZeroDivisionError:
                pass
        except IndexError:
            pass
        # 绘制所用轮廓
        if display:
            result_img = cv.drawContours(img, contours, -1, (158, 168, 3), 3)
            return position, result_img
        else:
            return position

    def getBlackRoute(self, img, optimize=False):
        """获取黑色胶布布置的路线中心线,并返回循迹排序坐标点

        Args:
            img (wild_img): 原生图像（要求背景干净）
            optimize (bool, optional): 是否开启路线优化. Defaults to False.

        Returns:
            route_list, skeleton:路线有序列表(绝对坐标)，骨线图
        """
        img = img.copy()
        img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
        # 高斯模糊滤波
        blur = cv.GaussianBlur(img_gray, (11, 11), 0)
        # 阈值得到黑色框
        _, binary = cv.threshold(blur, 100, 255, cv.THRESH_BINARY_INV)
        # 二值化
        binary[binary == 255] = 1
        # 细化算法,寻找黑框骨线
        skeleton0 = morphology.skeletonize(binary)
        skeleton = skeleton0.astype(np.uint8) * 255
        # 去除分叉点
        while optimize:
            endpoints = self.__endpoints_of_skeleton(skeleton)
            if len(endpoints) == 0:
                break
            for each in endpoints:
                skeleton[each[0], each[1]] = 0
        # 获取骨架线上的所有坐标
        indices = np.argwhere(skeleton)
        points = [tuple(index[::-1]) for index in indices]
        route_list = []
        tree = spt.cKDTree(data=points)
        route_list.append(points[0])
        del points[0]
        distances, indexs = tree.query(route_list[0], k=3)
        route_list.append(points[min(indexs[1:]) - 1])
        del points[min(indexs[1:] - 1)]
        length = len(points)
        for i in range(length):
            tree = spt.cKDTree(data=points)
            distances, indexs = tree.query(route_list[-1], k=1)
            route_list.append(points[indexs])
            del points[indexs]
            if len(points) == 1:
                route_list.append(points[0])
                break
        return route_list, skeleton
    def getBlackRectRoute(self,img,outlength,ignore=1):
        img = img.copy()
        img_gray = cv.cvtColor(img, cv.COLOR_BGR2GRAY)
        # 高斯模糊滤波
        blur = cv.GaussianBlur(img_gray, (11, 11), 0)
        # 阈值得到黑色框
        _, binary = cv.threshold(blur, 100, 255, cv.THRESH_BINARY_INV)
        cv.imshow("binary",binary)
        _,contours, hierarchy = cv.findContours(binary, cv.RETR_TREE, cv.CHAIN_APPROX_NONE)
        cv.drawContours(img,contours,-1,(0,255,0),3) 
        cv.imshow("contours",img)
        cv.waitKey(10)
        cnt = []
        # for each in contours:
        #     print(cv.contourArea(each))
        #     if cv.contourArea(each)<=41240+500 and cv.contourArea(each)>=41240-500 :
        #         cnt = each
        print(len(contours))
        cnt = contours[2]
        if len(cnt) == 0:
            return   (None,None),[],[]
        points=[]
        res_list = []
        orgin_list = []
        points,center = self.__enlargeContour(cnt,outlength)
        for i in range(len(points)):
            if i%ignore ==0 :
                res_list.append(points[i])
                orgin_list.append(cnt[i])
        return center,res_list,orgin_list
    
    
class Peripheral():
    #* 外设类，本类主要功能是实现和单片机之间的uart通信，以及PWM控制步进电机的实现
    Uart = None
    
    def uartInit(self,baud):
        self.Uart = serial.Serial("/dev/ttyAMA0", baud)

    def getUartData(self,wait=False):
        while wait:
            count = self.Uart.inWaiting()
            if count!=0 :
                break
        if wait == False :
            count = self.Uart.inWaiting()
            if count == 0:
                    return None
        data = self.Uart.read(count).decode('utf-8') 
        self.Uart.flushInput()
        return data
    
    def sendUartData(self,data):
        return self.Uart.write(f"{data}".encode('utf-8'))
